﻿//#include <stdio.h>
//#include <math.h>
//#include "Ellipsoid_Fitting.h"

//#define N 100

////double x[N],y[N],z[N];
//double a,b,c;
//double xc,yc,zc;

//Ellipsoid_fitting_Data_t Ellipsoid_fitting_Data;

//void ellipsoidFit(float *X, float *Y, float *Z, int n) 
//{
//	
//	float D[n][10],S[10][10],V[10][10];
//	float x_mean = 0,y_mean = 0,z_mean = 0;
//	int i,j;
//	
//	//计算坐标平均值
//	for (i = 0; i < n; i++) {
//	x_mean += X[i];
//	y_mean += Y[i];
//	z_mean += Z[i];
//	}
//	x_mean /= n;
//	y_mean /= n;
//	z_mean /= n;
//	
//		// 构造矩阵D
//		for (i = 0; i < n; i++) {
//		D[i][0] = pow(X[i] - x_mean, 2);
//		D[i][1] = (X[i] - x_mean) * (Y[i] - y_mean);
//		D[i][2] = (Y[i] - y_mean) * (Y[i] - y_mean);
//		D[i][3] = (X[i] - x_mean) * (Z[i] - z_mean);
//		D[i][4] = (Y[i] - y_mean) * (Z[i] - z_mean);
//		D[i][5] = pow(Z[i] - z_mean, 2);
//		D[i][6] = X[i] - x_mean;
//		D[i][7] = Y[i] - y_mean;
//		D[i][8] = Z[i] - z_mean;
//		D[i][9] = 1;
//		}
//		
//		// 构造矩阵S
//		for (i = 0; i < 10; i++) {
//			 for (j = 0; j < 10; j++) {
//					S[i][j] = 0;
//					for (int k = 9; k < n; k++) {
//							S[i][j] += D[k][i] * D[k][j];
//							}
//						}
//					}	
//		
//					
//					//对矩阵S进行特征值分解
//				//这里使用了高斯-约旦消元法来求解特征值和特征向量
//				//实际应用中，可以使用更高效的算法来求解特征值和特征向量
//				for (i = 0; i < 10; i++) {
//					 for (j = 0; j < 10; j++) {
//							if (i == j) {
//							 V[i][j] = 1;
//							} else {
//							 V[i][j] = 0;
//							}
//						}
//					}
//				
//				for (i = 0; i < 10; i++) {
//				int p = i;
//				float m = fabs(S[i][i]);
//					for (j = i + 1; j < 10; j++) 
//						{
//						 if (fabs(S[j][i]) > m) 
//							 {
//								p = j;
//								m = fabs(S[j][i]);
//							 }
//						}
//						
//						if (p != i) 
//							{
//                for (j = 0; j < 10; j++) 
//										{
//										float tmp = S[i][j];
//											S[i][j] = S[p][j];
//											S[p][j] = tmp;
//											tmp = V[i][j];
//											V[i][j] = V[p][j];
//											V[p][j] = tmp;
//										}
//              }
//							for (j = i + 1; j < 10; j++){
//                float f = S[j][i] / S[i][i];
//                for (int k = i + 1; k < 10; k++){
//								S[j][k] -= f * S[i][k];
//							  }
//								S[j][i] = 0;
//								for (int k = 0; k < 10; k++) {
//								V[j][k] -= f * V[i][k];
//								}
//							}
//						}
//						for (i = 9; i >= 0; i--) 
//								{
//								float s = 0;
//								for (j = i + 1; j < 10; j++) 
//										{
//										s += S[i][j] * D[n - 1][j];
//										}
//								D[n - 1][i] = (D[n - 1][i] - s) / S[i][i];
//								}
//								
//								//椭球半轴长度
//						    Ellipsoid_fitting_Data.Data_A = D[n - 1][0];
//								Ellipsoid_fitting_Data.Data_B = D[n - 1][2];
//								Ellipsoid_fitting_Data.Data_C = D[n - 1][5];
//								
//								//椭球中心坐标
//								Ellipsoid_fitting_Data.Data_X0 = x_mean - D[n - 1][6] / 2;
//								Ellipsoid_fitting_Data.Data_Y0 = y_mean - D[n - 1][7] / 2;
//								Ellipsoid_fitting_Data.Data_Z0 = z_mean - D[n - 1][8] / 2;
//						
//												
//}
//					


//void Ellipsoid_fitting_Process(float *x, float y[],float z[])
//{


//    ellipsoidFit(x, y, z, Fitting_NUM);


//}

#include "Ellipsoid_Fitting.h"

#define MATRIX_SIZE 6
#define u8 unsigned char
float m_matrix[MATRIX_SIZE][MATRIX_SIZE + 1];//系数矩阵
float solve[MATRIX_SIZE] = { 0 };//方程组的解对应最小二乘椭球拟合中的，a，b，c，d，e，f，

float m_result[MATRIX_SIZE];
int N = 0;//计算累计的采样点次数的

Ellipsoid_fitting_Data_t Ellipsoid_fitting_Data;

//取绝对值
float Abs(float a)
{
	return a < 0 ? -a : a;
}

int Matrix_Flag = 0;
//把矩阵系数全部清除为0
void ResetMatrix(void)
{
	if(Matrix_Flag == 0)
	{
	for (u8 row = 0; row < MATRIX_SIZE; row++)
	{
		for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
			m_matrix[row][column] = 0.0f;
	}
	Matrix_Flag = 1;
  }
}

//把输入的数据先生成矩阵的元素的总和
void CalcData_Input(float x, float y, float z)
{
	short V[MATRIX_SIZE + 1];
	N++;
	V[0] = y*y;
	V[1] = z*z;
	V[2] = x;
	V[3] = y;
	V[4] = z;
	V[5] = 1.0;
	V[6] = -x*x;
	//构建系数矩阵，并进行累加
	for (u8 row = 0; row < MATRIX_SIZE; row++)
	{
		for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
		{
			m_matrix[row][column] += V[row] * V[column];
		}
	}
	//b向量是m_matrix[row][6]
}

//化简系数矩阵，把除以N带上
void CalcData_Input_average()
{
	for (u8 row = 0; row < MATRIX_SIZE; row++)
	for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
		m_matrix[row][column] /= N;
	//b向量是m_matrix[row][6]
}

//显示出来系数矩阵和增广矩阵[A|b]
void DispMatrix(void)
{
	for (u8 row = 0; row < MATRIX_SIZE; row++)
	{
		for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
		{
			printf("%23f ", m_matrix[row][column]);
			if (column == MATRIX_SIZE - 1)
				printf("|");
		}
		printf("\r\n");
	}
	printf("\r\n\r\n");
}

//交换两行元素位置
void Row2_swop_Row1(int row1, int row2)
{
	float tmp = 0;
	for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
	{
		tmp = m_matrix[row1][column];
		m_matrix[row1][column] = m_matrix[row2][column];
		m_matrix[row2][column] = tmp;
	}
}

//用把row行的元素乘以一个系数k
void k_muiltply_Row(float k, int row)
{
	for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
		m_matrix[row][column] *= k;
}

//用一个数乘以row1行加到row2行上去
void Row2_add_kRow1(float k, int row1, int row2)
{
	for (u8 column = 0; column < MATRIX_SIZE + 1; column++)
		m_matrix[row2][column] += k*m_matrix[row1][column];
}


//列主元，第k次消元之前，把k行到MATRIX_SIZE的所有行进行冒泡排出最大，排序的依据是k列的元素的大小
void MoveBiggestElement_to_Top(int k)
{
	int row = 0, column = 0;

	for (row = k + 1; row < MATRIX_SIZE; row++)
	{
		if (Abs(m_matrix[k][k]) < Abs(m_matrix[row][k]))
		{
			Row2_swop_Row1(k, row);
		}
	}
}

//高斯消元法，求行阶梯型矩阵
u8 Matrix_GaussElimination(void)
{
	float k = 0;
	for (u8 cnt = 0; cnt < MATRIX_SIZE; cnt++)//进行第k次的运算，主要是针对k行以下的行数把k列的元素都变成0
	{
		//把k行依据k列的元素大小，进行排序
		MoveBiggestElement_to_Top(cnt);
		if (m_matrix[cnt][cnt] == 0)
			return(1);//返回值表示错误
		//把k行下面的行元素全部消成0，整行变化
		for (u8 row = cnt + 1; row < MATRIX_SIZE; row++)
		{
			k = -m_matrix[row][cnt] / m_matrix[cnt][cnt];
			Row2_add_kRow1(k, cnt, row);
		}
		DispMatrix();
	}
	return 0;
}

//求行最简型矩阵，即把对角线的元素全部化成1
void Matrix_RowSimplify(void)
{
	float k = 0;
	for (u8 row = 0; row < MATRIX_SIZE; row++)
	{
		k = 1 / m_matrix[row][row];
		k_muiltply_Row(k, row);
	}
	DispMatrix();
}

//求非齐次线性方程组的解
void Matrix_Solve(float* solve)
{
	for (short row = MATRIX_SIZE - 1; row >= 0; row--)
	{
		solve[row] = m_matrix[row][MATRIX_SIZE];
		for (u8 column = MATRIX_SIZE - 1; column >= row + 1; column--)
			solve[row] -= m_matrix[row][column] * solve[column];
	}
//	printf("  a = %f| b = %f| c = %f| d = %f| e = %f| f = %f ", solve[0], solve[1], solve[2], solve[3], solve[4], solve[5]);
//	printf("\r\n");
//	printf("\r\n");
}

float xx[Fitting_NUM];
//整个椭球校准的过程
void Ellipsoid_fitting_Process(float *x, float y[],float z[])
{
	u8 i;
	ResetMatrix();
	
//	for(i = 0; i < Fitting_NUM; i++)
//	{
//		xx[i]=x[i];
//	}
	
	//这里输入任意个点加速度参数，尽量在球面上均匀分布
	for(i = 0; i < Fitting_NUM; i++)
	{
	CalcData_Input(x[i], y[i], z[i]);
	}
	
	CalcData_Input_average();//对输入的数据到矩阵元素进行归一化
	DispMatrix();//显示原始的增广矩阵
	if (Matrix_GaussElimination())	//求得行阶梯形矩阵
		printf("the marix could not be solved\r\n");
	else
	{
		Matrix_RowSimplify();//化行最简形态
		Matrix_Solve(solve);//求解a,b,c,d,e,f

		float a = 0, b = 0, c = 0, d = 0, e = 0, f = 0;
		a = solve[0];
		b = solve[1];
		c = solve[2];
		d = solve[3];
		e = solve[4];
		f = solve[5];

		float X0 = 0, Y0 = 0, Z0 = 0, A = 0, B = 0, C = 0;
		
		X0 = -c / 2;
		Y0 = -d / (2 * a);
		Z0 = -e / (2 * b);
		
		A = sqrt(X0*X0 + a*Y0*Y0 + b*Z0*Z0 - f);
		B = A / sqrt(a);
		C = A / sqrt(b);
		
		Ellipsoid_fitting_Data.Data_X0 = X0;
		Ellipsoid_fitting_Data.Data_Y0 = Y0;
		Ellipsoid_fitting_Data.Data_Z0 = Z0;
		Ellipsoid_fitting_Data.Data_A = A;
		Ellipsoid_fitting_Data.Data_B = B;
		Ellipsoid_fitting_Data.Data_C = C;
		
		
//		printf("  ((x - x0) / A) ^ 2 + ((y - y0) / B) ^ 2 + ((z - z0) / C) ^ 2 = 1 Ellipsoid result as below：\r\n");
//		printf("\r\n");
//		printf("  X0 = %f| Y0 = %f| Z0 = %f| A = %f| B = %f| C = %f \r\n", X0, Y0, Z0, A, B, C);
	}
	
	
}


	
const Ellipsoid_fitting_Data_t *get_Ellipsoid_fitting_Data(void)
{
    return &Ellipsoid_fitting_Data;
}
